{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 仿生蛇基本运动控制\n",
    "\n",
    "## 硬件准备\n",
    "\n",
    "材料：\n",
    "\n",
    "功能模块F*1、T模块*5\n",
    "\n",
    "\n",
    "\n",
    "## 硬件连接\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "## 关节配置\n",
    "\n",
    "将硬件连接的关节ID次序更新到配置文件中\n",
    "\n",
    "\n",
    "\n",
    "配置完后启动设备\n",
    "\n",
    "\n",
    "# 代码说明\n",
    "\n",
    "### 导入库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!/usr/bin/python\n",
    "# -*- coding: UTF-8 -*-\n",
    "import sys;\n",
    "sys.path.append(\"..\")\n",
    "sys.path.append(\"../../\")\n",
    "from lib.BwRobotLib import BwRobotLib\n",
    "from lib.MiniBotControlLib import MiniBotControlLib\n",
    "from RobotSnake.SnakeRobot import SnakeRobot\n",
    "import keyboard\n",
    "import time\n",
    "import _thread"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 声明库对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#声明库对象\n",
    "robotlib = BwRobotLib()\n",
    "snake = SnakeRobot(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 连接设备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "连接成功\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#连接机器人设备\n",
    "robotlib.connectRobot(\"192.168.1.107\",55001)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "读取ID配置文件:\n",
      "#仿生蛇\n",
      "\n",
      "UP:30,31,32,33,34\n",
      "\n"
     ]
    }
   ],
   "source": [
    "#加载配置（兼容VS项目启动方式及文件直接启动方式）\n",
    "import os,inspect\n",
    "fileName = inspect.getframeinfo(inspect.currentframe()).filename\n",
    "folderPath     = os.path.dirname(os.path.abspath(fileName))\n",
    "cfile=folderPath+\"/config.txt\"\n",
    "robotlib.loadConfig(cfile)\n",
    "\n",
    "#配置操作臂\n",
    "snake.bindIds(robotlib.module_ids)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 参数设置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "#参数设置\n",
    "def handleSettingCmd(cmd):\n",
    "    print(\"请输入参数并回车:\")\n",
    "    parameter = robotlib.inputNum()\n",
    "   \n",
    "    if(cmd == '6'):\n",
    "        snake.setVelocity(parameter)\n",
    "    if(cmd == '7'):\n",
    "        snake.setSwingRange(parameter)\n",
    "    if(cmd == '8'):\n",
    "        snake.setSwingHz(parameter)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 键盘控制处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "#处理键盘\n",
    "def handleControlKeys(key):\n",
    "    isDown = key.event_type == 'down'\n",
    "    if not isDown:\n",
    "        snake.stop()\n",
    "        return\n",
    "\n",
    "    cmd = key.name\n",
    "    if not(cmd == '6' or cmd == '7' or cmd == '8' or cmd == '3'):\n",
    "        if not (snake.stopMoving_signal):\n",
    "            return\n",
    "\n",
    "    if(cmd == '1'):\n",
    "        snake.goForward()\n",
    "    if(cmd == '2'):\n",
    "        snake.goBack()\n",
    "    if(cmd == '3'):\n",
    "        snake.stop()\n",
    "    if(cmd == '4'):\n",
    "        snake.turnLeft()\n",
    "    if(cmd == '5'):\n",
    "        snake.turnRight()\n",
    "\n",
    "    if(cmd == '6' or cmd == '7' or cmd == '8'):\n",
    "        handleSettingCmd(cmd)\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 控制提示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "#控制提示\n",
    "def controlTips():\n",
    "    print(\"键盘控制命令 :\")\n",
    "    print(\"1 : 前进\")\n",
    "    print(\"2 : 后退\")\n",
    "    print(\"3 : 停止\")\n",
    "    print(\"4 : 左转\")\n",
    "    print(\"5 : 右转\")\n",
    "\n",
    "\n",
    "    print(\"-----------------------\")\n",
    "    print(\"0 :退出\")\n",
    "    \n",
    "    keyboard.hook(handleControlKeys)\n",
    "    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce\n",
    "    #helper.Wait(True)\n",
    "    keyboard.wait('0')\n",
    "    keyboard.unhook_all()\n",
    "    input(\"输入任意键回车（Enter）以继续.....\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 运行线程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "输入任意字符 Enter 开始初始化...\n",
      "停止\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "248"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#运行线程\n",
    "def snakeRun(threadName,arg):\n",
    "    snake.run()\n",
    "\n",
    "print(\"输入任意字符 Enter 开始初始化...\")\n",
    "snake.stop()\n",
    "_thread.start_new_thread(snakeRun,(\"runthread\",snake))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主入口"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "命令：\n",
      "1 : 控制\n",
      "2 : 设置速率参数\n",
      "3 : 设置振幅\n",
      "4 : 设置频率\n",
      "-----------------------\n",
      "0 : 退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： 1\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "键盘控制命令 :\n",
      "1 : 前进\n",
      "2 : 后退\n",
      "3 : 停止\n",
      "4 : 左转\n",
      "5 : 右转\n",
      "-----------------------\n",
      "0 :退出\n",
      "停止\n",
      "向前走\n",
      "停止\n",
      "向前走\n",
      "停止\n",
      "向后走\n",
      "停止\n",
      "向后走\n",
      "停止\n",
      "向后走\n",
      "停止\n",
      "停止\n",
      "停止\n",
      "左转\n",
      "停止\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "输入任意键回车（Enter）以继续..... 0\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "命令：\n",
      "1 : 控制\n",
      "2 : 设置速率参数\n",
      "3 : 设置振幅\n",
      "4 : 设置频率\n",
      "-----------------------\n",
      "0 : 退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： 2\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "请输入参数并回车:\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " 30\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "命令：\n",
      "1 : 控制\n",
      "2 : 设置速率参数\n",
      "3 : 设置振幅\n",
      "4 : 设置频率\n",
      "-----------------------\n",
      "0 : 退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： 4\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "请输入参数并回车:\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      " 32\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "命令：\n",
      "1 : 控制\n",
      "2 : 设置速率参数\n",
      "3 : 设置振幅\n",
      "4 : 设置频率\n",
      "-----------------------\n",
      "0 : 退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： 1\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "键盘控制命令 :\n",
      "1 : 前进\n",
      "2 : 后退\n",
      "3 : 停止\n",
      "4 : 左转\n",
      "5 : 右转\n",
      "-----------------------\n",
      "0 :退出\n",
      "停止\n",
      "向前走\n",
      "停止\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "输入任意键回车（Enter）以继续..... 0\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "命令：\n",
      "1 : 控制\n",
      "2 : 设置速率参数\n",
      "3 : 设置振幅\n",
      "4 : 设置频率\n",
      "-----------------------\n",
      "0 : 退出\n"
     ]
    }
   ],
   "source": [
    "while(1):\n",
    "    print(\"命令：\")\n",
    "    print(\"1 : 控制\")\n",
    "    print(\"2 : 设置速率参数\")\n",
    "    print(\"3 : 设置振幅\")\n",
    "    print(\"4 : 设置频率\")\n",
    "    print(\"-----------------------\")\n",
    "    print(\"0 : 退出\")\n",
    "   \n",
    "    cmd = robotlib.inputNum(int,\"请输入：\")\n",
    "    if(cmd == 1):\n",
    "       controlTips()\n",
    "    if(cmd == 2):\n",
    "       handleSettingCmd(6)\n",
    "    if(cmd == 3):\n",
    "       handleSettingCmd(7)\n",
    "    if(cmd == 4):\n",
    "       handleSettingCmd(8)\n",
    "    if(cmd == 0):\n",
    "       break"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
